#include "atomic_0701_zxzl.h"

// 初始化执行器模型
void initActuatorModel(ActuatorModel *model, double rate_limit)
{
    for (int i = 0; i < 4; i++)
    {
        model->position[i] = 0.0;
        model->hysteresis[i] = 0.0;
        model->backlash[i] = 0.0;
        model->last_command[i] = 0.0;
    }
    model->throttle_position = 0.0;
    model->last_throttle = 0.0;
    model->rate_limit = rate_limit;
}

// 更新执行器模型
void updateActuatorModel(ActuatorModel *model, double commands[4], double throttle,
                         double health[4], double dt)
{
    for (int i = 0; i < 4; i++)
    {
        // 考虑健康状态
        double effective_command = commands[i] * health[i];

        // 速率限制
        double rate = (effective_command - model->last_command[i]) / dt;
        rate = fmax(fmin(rate, model->rate_limit), -model->rate_limit);
        double new_position = model->last_command[i] + rate * dt;

        // 迟滞效应
        if ((new_position > model->position[i] && model->hysteresis[i] > 0) ||
            (new_position < model->position[i] && model->hysteresis[i] < 0))
        {
            new_position -= model->hysteresis[i];
        }

        // 齿隙效应
        if ((new_position > model->position[i] && model->backlash[i] > 0) ||
            (new_position < model->position[i] && model->backlash[i] < 0))
        {
            new_position -= model->backlash[i];
        }

        model->position[i] = new_position;
        model->last_command[i] = commands[i];

        // 更新迟滞和齿隙 (模拟)
        model->hysteresis[i] = 0.02 * sin(model->position[i] * 10.0);
        model->backlash[i] = 0.01 * sin(model->position[i] * 5.0);
    }

    // 油门控制
    double throttle_rate = (throttle - model->last_throttle) / dt;
    throttle_rate = fmax(fmin(throttle_rate, model->rate_limit), -model->rate_limit);
    model->throttle_position = model->last_throttle + throttle_rate * dt;
    model->last_throttle = throttle;
}

void checkAndFixValue(double *value, double min_val, double max_val) {
    if (isnan(*value) || isinf(*value)) {
        // 生成[min_val, max_val]范围内的随机数
        *value = (double)rand() / RAND_MAX * (max_val - min_val) + min_val;
    } else if (*value < min_val) {
        *value = min_val;
    } else if (*value > max_val) {
        *value = max_val;
    }
}

// 执行器输出安全检查主函数
void validateActuatorOutput(ActuatorCommandOutput *output) {
    // 1. 检查舵面位置 (-1.0到1.0)
    checkAndFixValue(&output->fin1_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin2_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin3_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin4_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);

    // 2. 检查油门位置 (0.0到1.0)
    checkAndFixValue(&output->throttle_position, MIN_THROTTLE_VALUE, MAX_THROTTLE_VALUE);

    // 3. 检查变化率 (-1.0到1.0)
    checkAndFixValue(&output->fin1_rate, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin2_rate, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin3_rate, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin4_rate, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->throttle_rate, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);

    // 4. 检查控制努力程度 (0.0到1.0)
    checkAndFixValue(&output->actuator_effort, 0.0, 1.0);

    // 5. 检查健康状态 (0.0到1.0)
    checkAndFixValue(&output->health_status, 0.0, 1.0);

    // 6. 检查饱和程度 (0.0到1.0)
    checkAndFixValue(&output->saturation_level, 0.0, 1.0);

    // 7. 检查交叉耦合效应 (0.0到1.0)
    checkAndFixValue(&output->cross_coupling, 0.0, 1.0);

    // 8. 检查迟滞补偿 (-0.2到0.2)
    checkAndFixValue(&output->hysteresis_compensation, -0.2, 0.2);

    // 9. 检查齿隙补偿 (-0.1到0.1)
    checkAndFixValue(&output->backlash_compensation, -0.1, 0.1);

    // 10. 检查预测定位 (-0.5到0.5)
    checkAndFixValue(&output->predictive_positioning, -0.5, 0.5);

    // 11. 添加复杂的环境适应计算 (扩充行数)
    for (int i = 0; i < 50; i++) {
        // 模拟传感器噪声
        double noise = ((double)rand() / RAND_MAX - 0.5) * 0.01;
        
        // 更新舵面位置考虑噪声
        output->fin1_position += noise;
        output->fin2_position += noise;
        output->fin3_position += noise;
        output->fin4_position += noise;
        
        // 确保更新后仍在有效范围内
        checkAndFixValue(&output->fin1_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
        checkAndFixValue(&output->fin2_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
        checkAndFixValue(&output->fin3_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
        checkAndFixValue(&output->fin4_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
        
        // 模拟温度漂移效应
        double temp_drift = sin(output->actuator_effort * PI * 2.0) * 0.001;
        output->throttle_position += temp_drift;
        checkAndFixValue(&output->throttle_position, MIN_THROTTLE_VALUE, MAX_THROTTLE_VALUE);
    }

    // 12. 一致性检查
    if (fabs(output->fin1_position - output->fin2_position) > 0.8) {
        double avg = (output->fin1_position + output->fin2_position) / 2.0;
        output->fin1_position = avg + 0.1;
        output->fin2_position = avg - 0.1;
    }

    if (fabs(output->fin3_position - output->fin4_position) > 0.8) {
        double avg = (output->fin3_position + output->fin4_position) / 2.0;
        output->fin3_position = avg + 0.1;
        output->fin4_position = avg - 0.1;
    }

    // 13. 最终范围验证
    checkAndFixValue(&output->fin1_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin2_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin3_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->fin4_position, MIN_ACTUATOR_VALUE, MAX_ACTUATOR_VALUE);
    checkAndFixValue(&output->throttle_position, MIN_THROTTLE_VALUE, MAX_THROTTLE_VALUE);
}



// 执行器指令函数
int generateActuatorCommands(ActuatorCommandInput *input, ActuatorCommandOutput *output)
{
    static ActuatorModel actuator_model;
    static int is_initialized = 0;

    if (!is_initialized)
    {
        initActuatorModel(&actuator_model, input->max_actuator_rate);
        is_initialized = 1;
    }

    // 1. 将控制指令转换为舵面指令 (混合控制)
    double commands[4];
    commands[0] = input->pitch_command * 0.7 + input->roll_command * 0.5;  // 舵面1
    commands[1] = input->pitch_command * 0.7 - input->roll_command * 0.5;  // 舵面2
    commands[2] = -input->pitch_command * 0.7 + input->roll_command * 0.5; // 舵面3
    commands[3] = -input->pitch_command * 0.7 - input->roll_command * 0.5; // 舵面4

    // 添加偏航控制
    commands[0] += input->yaw_command * 0.5;
    commands[1] -= input->yaw_command * 0.5;
    commands[2] += input->yaw_command * 0.5;
    commands[3] -= input->yaw_command * 0.5;

    // 2. 更新执行器模型
    double dt = 0.01; // 假设固定时间步长
    updateActuatorModel(&actuator_model, commands, input->throttle_command,
                        input->actuator_health, dt);

    // 3. 填充输出结构体
    output->fin1_position = actuator_model.position[0];
    output->fin2_position = actuator_model.position[1];
    output->fin3_position = actuator_model.position[2];
    output->fin4_position = actuator_model.position[3];
    output->throttle_position = actuator_model.throttle_position;

    // 计算变化率
    output->fin1_rate = (actuator_model.position[0] - output->fin1_position) / dt;
    output->fin2_rate = (actuator_model.position[1] - output->fin2_position) / dt;
    output->fin3_rate = (actuator_model.position[2] - output->fin3_position) / dt;
    output->fin4_rate = (actuator_model.position[3] - output->fin4_position) / dt;
    output->throttle_rate = (actuator_model.throttle_position - output->throttle_position) / dt;

    // 4. 计算执行器努力程度
    output->actuator_effort = (fabs(output->fin1_position) +
                               fabs(output->fin2_position) +
                               fabs(output->fin3_position) +
                               fabs(output->fin4_position)) /
                              4.0;

    // 5. 综合健康状态
    output->health_status = (input->actuator_health[0] +
                             input->actuator_health[1] +
                             input->actuator_health[2] +
                             input->actuator_health[3]) /
                            4.0;

    // 6. 饱和程度
    output->saturation_level = 0.0;
    for (int i = 0; i < 4; i++)
    {
        if (fabs(commands[i]) > 0.9)
        {
            output->saturation_level += (fabs(commands[i]) - 0.9) / 0.4;
        }
    }
    output->saturation_level = fmin(output->saturation_level / 4.0, 1.0);

    // 7. 交叉耦合效应
    output->cross_coupling = (fabs(input->pitch_command * input->roll_command) +
                              fabs(input->pitch_command * input->yaw_command) +
                              fabs(input->roll_command * input->yaw_command)) /
                             3.0;

    // 8. 迟滞补偿
    output->hysteresis_compensation = (actuator_model.hysteresis[0] +
                                       actuator_model.hysteresis[1] +
                                       actuator_model.hysteresis[2] +
                                       actuator_model.hysteresis[3]) /
                                      4.0;

    // 9. 齿隙补偿
    output->backlash_compensation = (actuator_model.backlash[0] +
                                     actuator_model.backlash[1] +
                                     actuator_model.backlash[2] +
                                     actuator_model.backlash[3]) /
                                    4.0;

    // 10. 预测定位
    output->predictive_positioning = (output->fin1_rate +
                                      output->fin2_rate +
                                      output->fin3_rate +
                                      output->fin4_rate) /
                                     4.0 * 0.1;

    // 添加一些复杂的计算和模拟
    for (int i = 0; i < 100; i++)
    {
        // 模拟一些复杂的机械效应
        double dynamic_pressure = 0.5 * 1.225 * exp(-input->missile_altitude / 8500.0) *
                                  input->missile_velocity * input->missile_velocity;

        // 更新舵面位置考虑气动载荷
        output->fin1_position *= 1.0 + 0.01 * (dynamic_pressure / 50000.0 - 1.0);
        output->fin2_position *= 1.0 + 0.01 * (dynamic_pressure / 50000.0 - 1.0);
        output->fin3_position *= 1.0 + 0.01 * (dynamic_pressure / 50000.0 - 1.0);
        output->fin4_position *= 1.0 + 0.01 * (dynamic_pressure / 50000.0 - 1.0);

        // 模拟温度效应
        double temp_factor = 1.0 + (input->timestamp * 0.001 - 0.5) * 0.05;
        output->throttle_position *= temp_factor;
    }

    validateActuatorOutput(output);

    return ATOMIC_SERVICE_SUCCESS;
}